/**
 * @file NewProtocolCurtisDHL.cpp
 * @author SoBigDog (luke@seer-robotics.com)
 * @brief 
 * @version 0.1
 * @date 2019-11-13
 * 
 * @copyright Copyright (c) 2019
 * 
 */
#include "NewProtocolCurtisDHL.h"

int16_t CNewProtocolCurtisDHL::_speedVal = 0;
int16_t CNewProtocolCurtisDHL::_forkVel = 0;
int16_t CNewProtocolCurtisDHL::_lower_flag = 0;
int16_t CNewProtocolCurtisDHL::_lift_flag = 0;
int16_t CNewProtocolCurtisDHL::_forwardVel = 0;
int16_t CNewProtocolCurtisDHL::_go_ahead_flag = 0;
int16_t CNewProtocolCurtisDHL::_go_back_flag = 0;

CNewProtocolCurtisDHL::CNewProtocolCurtisDHL()
{
	_encoderLine = 1024;
}

/**
 * @brief 参见ProtocolDriver.h
 * 
 * @param ct 
 * @param val 
 * @return true 
 * @return false 
 */
bool CNewProtocolCurtisDHL::encode(motorDriverCmdType ct, int32_t val)
{
	if (DRV_CMD_TARGET_SPEED == ct) {
		_speedVal = (int16_t)(val * 60.0f / (4.0f * _encoderLine));
		return false;
	} else if (DRV_CMD_TARGET_POS == ct) {
		uint8_t driveRawDir;
		uint16_t driveRawSpeed;
		uint16_t steerRawAngle;
		uint8_t fork_ctrl_byte = 0;

		driveRawDir = (_speedVal < 0) ? 0x02: 0x01;
		driveRawSpeed = (_speedVal < 0) ? -_speedVal : _speedVal;
		fork_ctrl_byte = ((_lift_flag ? 1 : 0) << 6) + ((_lower_flag ? 1 : 0) << 7);

		steerRawAngle = val;
	
		CanTxMsg msg;
		msg.StdId = 0x231;
		msg.IDE = CAN_Id_Standard;
		msg.RTR = CAN_RTR_Data;
		msg.DLC = 8;
		msg.Data[0] = driveRawDir | fork_ctrl_byte;
		msg.Data[1] = driveRawSpeed >> 8;
		msg.Data[2] = driveRawSpeed & 0xFF;
		msg.Data[3] = 0x01;									//acc
		msg.Data[4] = 0xf4;									//acc
		msg.Data[5] = 0x01;
		msg.Data[6] = 0xf4;
		if (_go_ahead_flag) {
			msg.Data[7] = 0x80;
		} else if (_go_back_flag) {
			msg.Data[7] = 0x40;
		} else {
			msg.Data[7] = 0x00;
		}
		_canRouter->putMsg(msg);

		msg.StdId = 0x233;
		msg.Data[0] = 0x0b;
		msg.Data[1] = 0xb8;
		msg.Data[2] = (-(steerRawAngle * 10)) >> 8;
		msg.Data[3] = (-(steerRawAngle * 10)) & 0xff;

		if (_lift_flag || _lower_flag) {
			if (_lower_flag) {
				msg.Data[4] = (int16_t)(_forkVel * 32.767f) >> 8;
				msg.Data[5] = (int16_t)(_forkVel * 32.767f) & 0xFF;
			} else {
				msg.Data[4] = 0x00;	
				msg.Data[5] = 0x00;	
			}

			if (_lift_flag) {
				_forkVel = _forkVel * 4;
				msg.Data[6] = _forkVel >> 8;	
				msg.Data[7] = _forkVel & 0xFF;	
			} else {
				msg.Data[6] = 0x00;	
				msg.Data[7] = 0x00;	
			}
		} else {
			if (_go_ahead_flag || _go_back_flag) {
				msg.Data[4] = 0x00;
				msg.Data[5] = 0x00;
				msg.Data[6] = (_forwardVel * 4) >> 8;	
				msg.Data[7] = (_forwardVel * 4) & 0xFF;	
			} else {
				msg.Data[4] = 0x00;
				msg.Data[5] = 0x00;
				msg.Data[6] = 0x00;	
				msg.Data[7] = 0x00;	
			}
		}
		_canRouter->putMsg(msg);
	} else if (DRV_CMD_FORK_VEL == ct) {
		_forkVel = val;
		if (_forkVel > 0) {
			_lift_flag = true;
			_lower_flag = false;
		} else if (_forkVel < 0) {
			_lift_flag = false;
			_lower_flag = true;
			_forkVel = -_forkVel;
		} else if (_forkVel == 0) {
			_lift_flag = false;
			_lower_flag = false;
		}

		return false;
	} else if (DRV_CMD_FORK_FORWARD_STATE == ct) {
		_forwardVel = val;
		if (_forwardVel > 0) {
			_go_ahead_flag = true;
			_go_back_flag = false;
		} else if (_forwardVel < 0) {
			_go_ahead_flag = false;
			_go_back_flag = true;
			_forwardVel = -_forwardVel;
		} else if (_forwardVel == 0) {
			_go_ahead_flag = false;
			_go_back_flag = false;
		}
	}
	return true;
}

/**
 * @brief 参见ProtocolDriver.h
 * 
 * @param ct 
 * @param val 
 * @param canMsg 
 * @return int 
 */
int CNewProtocolCurtisDHL::decode(motorDriverCmdType (&ct)[10], int32_t (&val)[10], const CanRxMsg& canMsg)
{
	volatile int16_t rawSpeed = 0;
	volatile int16_t rawAngle = 0;
	uint8_t wheel_error = 0;
	uint8_t driver_stute = 0;
	uint8_t steer_error = 0;

	rawSpeed = (canMsg.Data[0] << 8) + canMsg.Data[1];
	rawAngle = (canMsg.Data[2] << 8) + canMsg.Data[3];
	driver_stute = canMsg.Data[5];
	wheel_error = canMsg.Data[6];
	steer_error = canMsg.Data[7];	

	if (wheel_error == 0x61) {
		wheel_error = 0;
	}

	if (_motorType == 1) {
		ct[0] = DRV_CMD_ACTURAL_SPEED;
		ct[1] = DRV_CMD_AUTO_MODE;
		ct[2] = DRV_CMD_DRI_ERR;
		ct[3] = DRV_CMD_DRI_ERRCODE;

		val[0] = rawSpeed;
		val[1] = (driver_stute & 0x02) ? 0 : 1;
		val[2] = (wheel_error > 0) ? 1 : 0;
		val[3] = wheel_error;

		return 4;
	} else if (_motorType == 2) {
		ct[0] = DRV_CMD_ACTURAL_POS;
		ct[1] = DRV_CMD_AUTO_MODE;
		ct[2] = DRV_CMD_DRI_ERR;
		ct[3] = DRV_CMD_DRI_ERRCODE;

		val[0] = (-rawAngle) / 10.0f;
		val[1] = (driver_stute & 0x02) ? 0 : 1;
		val[2] = (steer_error > 0) ? 1 : 0;
		val[3] = steer_error;

		return 4;
	} else if (_motorType == 6) {
		uint8_t forkstate = canMsg.Data[6];

		ct[0] = DRV_CMD_FORK_FORWARD_STATE;

		if ((forkstate & 0x02) == 0x00) {
			val[0] = 1;
		} else if ((forkstate & 0x04) == 0x00) {
			val[0] = 2;
		} else {
			val[0] = 0;
		}
		return 1;
	} else {
		return 0;
	}
}

/**
 * @brief 参见ProtocolDriver.h
 * 
 * @param msg 
 * @return true 
 * @return false 
 */
bool CNewProtocolCurtisDHL::lifeguard(CanTxMsg& msg)
{
	if (_motorType == 1) {
		msg.StdId = 0x232;
		msg.IDE = CAN_Id_Standard;
		msg.RTR = CAN_RTR_Data;
		msg.DLC = 1;
		msg.Data[0] = 0x05;
		return true;
	} else {
		return false;
	}
}
